      program collocation_test
!     Solution of two-point BVP for ODE by collocation method with Bernstein polynomials
      use prec
      use bernstein
      use jacobian ! for non-linear problems
      use collocation
      use sparse
      use exact
      implicit none

      real(dp) :: a,b  ! interval
      integer,parameter :: n = 40 ! Order of polyniomial
      real(dp), dimension(0:n) :: nd ! nodes (values in interval [a,b]), there are n+1 nodes!!, where n is order of Bernstein polinomial 
      real(dp), dimension(1:n-1) :: f ! RHS vector
      real(dp), dimension(1:n-1,1:n-1) :: K ! System matrix from collocation discretization
      real(dp), dimension(0:n) :: beta ! SOLUTION! Vector containing coefs for Bernstein polynomial approximation, and correction vecotr in Newton iteration
      real(dp), dimension(1:n-1) :: delta ! Correction vector in Newton iterations
      real(dp), dimension(0:n) :: u,uex!,abser

      integer :: i,j 
      integer :: it
      integer ,parameter :: niter = 6 ! Maximum number of Newton iterations
      real(dp), parameter :: tol = 1e-5 ! If the norm of correction vector is smaller than 'tol', exit Newton iterations before 'niter' iterations finish.
      real(dp), parameter :: e = EXP(1.0_dp) ! Euler constant - used for BC in some problems
      real(dp) :: x ! temporary to store nodal location
      real(dp) :: dist,mv

!     Initialize beta array
!      beta = 0.0_dp

!     Define solutio interval [a,b]
       a = 0.0_dp
       b = 0.9_dp

!     Boundary conditions (expressed in terms of beta coefficients) 
      beta(0) = 1.0_dp
      beta(n) = 32.725_dp
!
      delta = 0.0_dp

!     Form the vector of nodes in [a,b] interval
      nd = bernstein_basis_nodes(n,a,b)
!      write(*,*) (nd(i), i=0,n,1)

!     initialize solution...we need good initial solution for Newton method!!!
      do i=1,n-1,1 ! we don't evaluate at 0 and n because it's covered by BC's of BVP
      x = nd(i) 
      dist = (x-a)/(b-a)
      mv = bernstein_basis_fun_maxval(i,n,a,b)
      beta(i) = ( beta(0)+dist*(beta(n)-beta(0)) ) !* mv <- inclue this for better initial condition for this prob
      enddo
!      write(*,*) 'Initial beta coefficients :'
!      call prtmtx(1,n+1,beta) 

!     CHECK Numerical solution at collocation points
      do i=0,n,1
        x = nd(i) ! n+1 collocation points, but it doesnt have to be these point, we can take as much points as we like in interval [a,b]
        uex(i) = Bernstein_interpolant(beta,n,a,b,x)
!        write(*,*) u(i)
      enddo
!     Write solution 
!      write(*,*) 'Solution vector u :'
!      call prtmtx(1,n+1,u) 

!     For non-linear problems 
!     We solve in following steps using Newton's algorithm:
!     1. Solve linear system: DF(xi) * Dx = -f(xi)
!     2. x(i+1) = x(i) + Dx
      do it = 1,niter !%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

!     Evaluate RHS at nodal positions to form RHS vector
      do i=1,n-1,1 ! we don't evaluate at 0 and n because it's covered by BC's of BVP
      x = nd(i) 
      f(i) = -evaluate_fun(x,a,b,n,beta)
      enddo
!      write(*,'(a)') 'RHS vector:'
!      write(*,*) (f(i), i=1,n-1,1)


!     Form system matrix A(i,j) evaluating LHS differential operator at i-th point on j-th basis function
      do i=1,n-1,1
        x = nd(i)
        do j=1,n-1,1 ! j define serial order of basis function, we don't need 0, and n because of BC's
!         K(i,j) entry is obtained by applying LHS differential operator on jth basis function at ith collocation point x
          K(i,j) = evaluate_jac(x,j,a,b,n,beta)
        enddo
      enddo

!     Write System matrix 
!      write(*,*) 'Matrix K :'
!      call prtmtx(n-1,n-1,K) 

!     Solve system
      delta = solve(K,f,n-1)
!      write(*,*) 'beta coefficients :'
!      call prtmtx(1,n+1,beta) 

!     Update solution
      beta(1:n-1) = beta(1:n-1) + delta(1:n-1)

!     Exit Newton iterations if norm of the correction vector is below 'tol'
      if(sum(delta*delta) < tol) exit

      end do ! after niter, non-linear solve %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

!     Find error in approximation ->>>>>>>
!     Numerical solution at collocation points
      do i=0,n,1
        x = nd(i) ! n+1 collocation points, but it doesnt have to be these point, we can take as much points as we like in interval [a,b]
        u(i) = Bernstein_interpolant(beta,n,a,b,x)
      enddo

!     Write solution 
!#      write(*,*) 'Solution vector u :'
!#      call prtmtx(1,n+1,u) 

!     find exact solution at collocation points
      do i=0,n,1
        x = nd(i) 
!        uex(i) = exact_solution(x)
      enddo

!#      write(*,'(a)') 'Exact solution uex: '
!#      call prtmtx(1,n+1,uex) 
!#      write(*,'(2x,e13.6)') (uex(i),i=0,n,1)
!      abser =  abs(u-uex)
      write(*,'(a,i2,a)') 'Node, initial solution, and collocation solution after ',niter,' Newton iterations.'
!#      call prtmtx(1,n+1,abser) 
      write(*,'(2x,e13.6,1x,e13.6,1x,e13.6)') (nd(i),uex(i),u(i),i=0,n,1)
!@      write(*,'(2x,e13.6)') (uex(i),i=0,n,1)
!                                 <<<<<<<<-
      stop
      end program 

      subroutine prtmtx(m,n,A)
!    
!     print matrix A
!
      use prec
      implicit none
      integer, intent(in) :: m,n
      real(dp),dimension(m,n), intent(in) :: A

!     Locals
      integer :: i,j

      do i=1,m
!               Ovde VVV pise 10 jer je n=9, dakle uvek stavi n+1
        write(*,'(2x,10(e13.6,2x))') (A(i,j),j=1,n)
      enddo

      return
      end subroutine
